#ifndef KINEMATICS_H
#define KINEMATICS_H

#include <vector>
#include <iostream>
#include <QVector>

#define PI 3.141592653589793
#define RAD2DEG 57.295779513082323
#define DEG2RAD 0.017453292519943
namespace CIRT_LBR {

class Transform
{
public:
    Transform();
    Transform(double _nx,double _ny,double _nz,
              double _ox,double _oy,double _oz,
              double _ax,double _ay,double _az,
              double _px,double _py,double _pz);
    Transform(const Transform& T);
    Transform& operator=(const Transform& T);
    Transform& operator-=(const Transform& T);
    double abs();
    Transform operator*(const Transform& T);
    friend std::ostream& operator<<(std::ostream& out, const Transform& T);
    void toeulerangle();
    double nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;
};

Transform LBR_fk(const QVector<double> &Q);
QVector<QVector<double> > LBR_ik(const Transform& T);
QVector<QVector<double> > LBR_inverse(const Transform& T, double* q_sols,double * q);
std::vector<std::pair<double,double> > phase_shift_eq(const double& a,const double& b, const double& c);
bool isequal(const QVector<double> &a, const QVector<double> &b);
void sincosround(double& value);
double maxdistance(const QVector<double> &joint1, const QVector<double> &joint2);
QVector<float> nearstsloution(const QVector<QVector<double> > sluts, const QVector<double> current);

}
#endif // KINEMATICS_H
